#include "Grapher.h"

#include "gnuplot_i.h"
#include "470bot.h"


using std::cout;
using std::endl;

Grapher::Grapher(void)
{
	out.open("graph.gpi");
	file = "graph.gpi";

	initGraphFile();

	currentVectors = 0;
	vectorsBeforeFrame = -1;
}

Grapher::Grapher(std::string filename,
				 double xmin, 
				 double xmax, 
				 double ymin, 
				 double ymax, 
				 int vectorsBeforeFrame, 
				 bool unset,
				 std::string title){
	out.open(filename.c_str());
	file = filename;
	
	if(!out.is_open()){
		cout << "Error opening file.  Switching to defualt." << endl;
		out.open("graph.gpi");
		file = "graph.gpi";
	}
	initGraphFile(xmin, xmax, ymin, ymax, title);
	parseWorldData();

	plot = new Gnuplot();

	time = 50.0;
	currentVectors = 0;
	this->vectorsBeforeFrame = vectorsBeforeFrame;
	this->unset = unset;
	staticCommand.clear();
	vectorsCommand.clear();
	standard.clear();
}

Grapher::~Grapher(void)
{
	closeFile();
}

void Grapher::startNewFile(std::string filename, 
						   double xmin, 
						   double xmax, 
						   double ymin, 
						   double ymax, 
						   int vectorsBeforeFrame,
						   bool unset,
						   std::string title){
	plot->~Gnuplot();
	plot = new Gnuplot();
							   
	closeFile();
	out.open(filename.c_str());
	file = filename;

	if(!out.is_open()){
		cout << "Error opening file.  Switching to defualt." << endl;
		out.open("graph.gpi");
		file = "graph.gpi";
	}

	staticCommand.str("");
	staticCommand.clear();
	vectorsCommand.str("");
	vectorsCommand.clear();
	standard.str("");
	standard.clear();

	time = 50.0;
	initGraphFile(xmin, xmax, ymin, ymax, title);
	parseWorldData();

	this->vectorsBeforeFrame = vectorsBeforeFrame;
	currentVectors = 0;
	this->unset = unset;
}

void Grapher::closeFile(){
	if(out.is_open())
		out.close();
}

void Grapher::addVector(Vector2 from, Vector2 to, int linetype, int width){
	if(!unset){
	vectorsCommand << "set arrow from " << from.x << "," << from.y
					   << " to " << to.x << "," << to.y
					   << " lt " << linetype << " lw " << width << endl;
	}else{
	out << "set arrow from " << from.x << "," << from.y
		<< " to " << to.x << "," << to.y
		<< " lt " << linetype << " lw " << width << endl;
	}
	
	if(vectorsBeforeFrame > 0){
		currentVectors++;
		if(currentVectors >= vectorsBeforeFrame){
			writeFrame();
			currentVectors = 0;
		}
	}
}

void Grapher::writeFrame(bool clear){
	out << staticCommand.str();

	if(!unset){
		out << vectorsCommand.str();
	}
	out << "plot '-' with lines" << endl << "0 0 0 0" << endl << "e" << endl;
	if(time > 0.0)
		out << "pause .01" << endl;

	if(clear)
		cmd("unset arrow");
}

void Grapher::displayAnimation(){
	std::ostringstream cmdstr;
	cmdstr << "load \"" << file << "\"" << endl;
	(*plot) << cmdstr.str();
}

void Grapher::cmd(std::string cmdstr){
	out << cmdstr << endl;
}

void Grapher::setMaxVectors(int maxVectors){
	vectorsBeforeFrame = maxVectors;
}

void Grapher::setUnSet(bool unset){
	this->unset = unset;
}

void Grapher::setTime(double time){
	this->time = time;
}

void Grapher::clearVertexBuffer(){
	writeFrame();
	currentVectors = 0;
	vectorsCommand.str("");
	vectorsCommand.clear();
}

void Grapher::WriteOutFinal(string Filename, std::vector<double*> rays){
	std::ofstream output;
	std::ostringstream writeOut;

	for(int i = 0; i < rays.size(); i ++){
		writeOut << rays[i][0] << " " << rays[i][1] << " " << rays[i][2] << " " << rays[i][3] << endl;
	}

	output.open(Filename);
	output<<writeOut.str();
	output.close();

	out << staticCommand.str();

	out << "plot \"" << Filename << "\" using 1:2:3:4 with vectors lt 7 lw 2" << endl;
}

void Grapher::initGraphFile(double xmin, double xmax, double ymin, double ymax, std::string title){
	if(!out.is_open()) return;
	
	std::ostringstream writeOut;

	writeOut << "clear" <<endl;
	writeOut << "reset" <<endl;
	writeOut << "set title " << title << std::endl;
	writeOut << "set xrange [" << xmin << ":" << xmax << "]" << endl;
	writeOut << "set yrange [" << ymin << ":" << ymax << "]" << endl;
	writeOut << "unset key" << endl;
	writeOut << "set size square" << endl;
	writeOut << "set pointsize 1.0" << endl;
	writeOut << "set xtics 8" << endl;
	writeOut << "set ytics 8" << endl;
	writeOut << "set grid" << endl;
	//writeOut << "plot '-' with lines " << endl << "0 0 0 0" << endl << "e" << endl;
	//writeOut << "pause .05" << endl;

	out << writeOut.str();
}

void Grapher::parseWorldData(){
	World* w = World::instance();
	
	std::vector<obstacle_t> obs = w->obstacles();
	std::vector<team_t> teams = w->teams();
	obstacle_t* ob = NULL;
	team_t* team = NULL;
	for(int i = 0; i <  obs.size(); i ++){
		ob = &obs[i];
		for(int j = 0; j < ob->size-1; j ++){
			staticCommand << "set arrow from " << ob->o_corner[j][0] << "," << ob->o_corner[j][1]
							 << " to " << ob->o_corner[j+1][0] << "," << ob->o_corner[j+1][1]
							 << " nohead lt -1 lw 3" << endl;
		}
		staticCommand << "set arrow from " << ob->o_corner[ob->size-1][0] << "," << ob->o_corner[ob->size-1][1]
							 << " to " << ob->o_corner[0][0] << "," << ob->o_corner[0][1]
							 << " nohead lt -1 lw 3" << endl;
	}

	int teamcolor;
	for(int i = 0; i < teams.size(); i++){
		team = &teams[i];
		if(team->color.compare("red") == 0){
			teamcolor = 1;
		}
		else if(team->color.compare("green") == 0){
			teamcolor = 2;
		}
		else if(team->color.compare("blue") == 0){
			teamcolor = 3;
		}
		else if(team->color.compare("purple") == 0){
			teamcolor = 4;
		}
		else continue;

		for(int j = 0; j < 3; j ++){
			staticCommand << "set arrow from " << team->base_corner[j][0] << "," << team->base_corner[j][1]
						  << " to " << team->base_corner[j+1][0] << "," << team->base_corner[j+1][1]
						  << " nohead lt " << teamcolor  << " lw 3" << endl;
		}
		staticCommand << "set arrow from " << team->base_corner[3][0] << "," << team->base_corner[3][1]
					  << " to " << team->base_corner[0][0] << "," << team->base_corner[0][1]
					  << " nohead lt " << teamcolor << " lw 3" << endl;
	}

	//standard << "plot '-' with points ls 4" << endl;
	
	Vector2 p;
	p.x = std::floor(w->tanks().at(0).pos[0]) + .5;
	p.y = std::floor(w->tanks().at(0).pos[1]) + .5;

	drawBox(p, 1);
	
	for (int i = 0; i < w->other_tanks().size(); i++) {	
		if(w->other_tanks()[i].color.compare("red") == 0){
			teamcolor = 1;
		}
		else if(w->other_tanks()[i].color.compare("green") == 0){
			teamcolor = 2;
		}
		else if(w->other_tanks()[i].color.compare("blue") == 0){
			teamcolor = 3;
		}
		else if(w->other_tanks()[i].color.compare("purple") == 0){
			teamcolor = 4;
		}

		p.x = std::floor(w->other_tanks().at(i).pos[0]) + .5;
		p.y = std::floor(w->other_tanks().at(i).pos[1]) + .5;

		drawBox(p, teamcolor);
		//standard << w->other_tanks().at(i).pos[0] << " " << w->other_tanks().at(i).pos[1] << endl;
	}

	//standard << "e" << endl;
	//standard << "pause .05" << endl;


	for (int i = 0; i < w->flags().size(); i++){
		if(w->flags()[i].color.compare("red") == 0){
			teamcolor = 1;
		}
		else if(w->flags()[i].color.compare("green") == 0){
			teamcolor = 2;
		}
		else if(w->flags()[i].color.compare("blue") == 0){
			teamcolor = 3;
		}
		else if(w->flags()[i].color.compare("purple") == 0){
			teamcolor = 4;
		}
		p.x = floor(w->flags()[i].pos[0])+.5;
		p.y = floor(w->flags()[i].pos[1])+.5;

		drawTriangle(p, teamcolor);
	}

	
	std::cout << staticCommand.str();
	//out << staticCommand.str();
}

void Grapher::drawBox(Vector2 pos, int color){
	staticCommand << "set arrow from " << pos.x-.5 << "," << pos.y-.5
				  << " to " << pos.x+.5 << "," << pos.y-.5
				  << " nohead lt " << color << " lw 1" << endl;
	staticCommand << "set arrow from " << pos.x+.5 << "," << pos.y-.5
				  << " to " << pos.x+.5 << "," << pos.y+.5
				  << " nohead lt " << color << " lw 1" << endl;
	staticCommand << "set arrow from " << pos.x+.5 << "," << pos.y+.5
				  << " to " << pos.x-.5 << "," << pos.y+.5
				  << " nohead lt " << color << " lw 1" << endl;
	staticCommand << "set arrow from " << pos.x-.5 << "," << pos.y+.5
				  << " to " << pos.x-.5 << "," << pos.y-.5
				  << " nohead lt " << color << " lw 1" << endl;
}

void Grapher::drawTriangle(Vector2 pos, int color){
	staticCommand << "set arrow from " << pos.x << "," << pos.y-1.5
				  << " to " << pos.x+1.5 << "," << pos.y+1.5
				  << " nohead lt " << color << " lw 1" << endl;
	staticCommand << "set arrow from " << pos.x+1.5 << "," << pos.y+1.5
				  << " to " << pos.x-1.5 << "," << pos.y+1.5
				  << " nohead lt " << color << " lw 1" << endl;
	staticCommand << "set arrow from " << pos.x-1.5 << "," << pos.y+1.5
				  << " to " << pos.x << "," << pos.y-1.5
				  << " nohead lt " << color << " lw 1" << endl;
}